using System;
using System.Collections.Generic;
using System.Text;
using System.ComponentModel;
using Microsoft.Dss.Core.Attributes;

namespace VEXRobotics
{
    #region VEX Startup
    /// <summary>
    /// Set the configuration of the controller
    /// </summary>
    [DataContract]
    public class VEXSetConfiguration : VEXCommand
    {
        /// <summary>
        /// Default constructor
        /// Uses 11 bytes for the data
        /// 
        /// Offset  Usage
        /// 0-1     Input/output mask
        /// 2       Number of analog ports
        /// 3       Bitmask for encoders
        /// 4       Bitmask for sonar
        /// 5-10    Port for the sonar device
        /// </summary>
        public VEXSetConfiguration()
            : base(VEXHelper.VEXCommandCode.ConfigurationSetCmd, 11)
        {
        }

        /// <summary>
        /// Default constructor for a state
        /// </summary>
        /// <param name="state">State to calculate information from</param>
        public VEXSetConfiguration(VEXState state)
            : base(VEXHelper.VEXCommandCode.ConfigurationSetCmd, 11)
        {
            // Set the direction of the IO ports
            SetUShort(0, CalcInputMask(state));

            // Set the number of analog ports
            SetSByte(2, CalcAnalog(state));

            // Set the encoder bits
            SetSByte(3, CalcEncoder(state));

            // Set the sonar port information
            CalcSonar(state);
        }

        /// <summary>
        /// Calculate the direction mask for the IO ports
        /// </summary>
        /// <param name="state">State to calculate information from</param>
        /// <returns>Integer with a bit set for input (cleared for output)</returns>
        private int CalcInputMask(VEXState state)
        {
            int inputMask = 0;
            int mask = 0x0001;

            foreach (PortSensorData s in state.PortSensors.Sensors)
            {
                // Input is bit set
                // Output is bit cleared
                switch (s.Type)
                {
                    case SensorDefinition.SensorType.None:
                        inputMask |= mask;
                        break;
                    case SensorDefinition.SensorType.Encoder:
                        break;

                    case SensorDefinition.SensorType.Potentiometer:
                        inputMask |= mask;
                        break;
                    case SensorDefinition.SensorType.Gyro:
                        inputMask |= mask;
                        break;
                    case SensorDefinition.SensorType.Accelerometer:
                        inputMask |= mask;
                        break;
                    case SensorDefinition.SensorType.LineTracker:
                        inputMask |= mask;
                        break;
                    case SensorDefinition.SensorType.AnalogInput:
                        inputMask |= mask;
                        break;
                    case SensorDefinition.SensorType.DigitalInput:
                        inputMask |= mask;
                        break;
                    case SensorDefinition.SensorType.DigitalOutput:
                        break;

                    case SensorDefinition.SensorType.Button:
                        inputMask |= mask;
                        break;
                    case SensorDefinition.SensorType.Limit:
                        inputMask |= mask;
                        break;

                    case SensorDefinition.SensorType.Sonar:
                        break;
                    case SensorDefinition.SensorType.LightSensor:
                        inputMask |= mask;
                        break;
                }

                // Shift the mask bit
                mask <<= 1;
            }
            return inputMask;
        }

        /// <summary>
        /// Calculate the number of analog ports
        /// </summary>
        /// <param name="state">State to calculate information from</param>
        /// <returns>Interger with the total number of analog devices.
        ///          NOTE:  Analog ports must start at port 1 and be contiguous.
        ///          (This is a PIC limitation.)</returns>
        public static int CalcAnalog(VEXState state)
        {
            // Spin through all of the ports
            int numAnalog = 0;
            foreach (PortSensorData s in state.PortSensors.Sensors)
            {
                // See if it is an analog port usage
                if (s.Type == SensorDefinition.SensorType.Accelerometer ||
                    s.Type == SensorDefinition.SensorType.AnalogInput ||
                    s.Type == SensorDefinition.SensorType.Gyro ||
                    s.Type == SensorDefinition.SensorType.LightSensor ||
                    s.Type == SensorDefinition.SensorType.LineTracker ||
                    s.Type == SensorDefinition.SensorType.Potentiometer
                    )
                {
                    // Increment the number of analog ports
                    ++numAnalog;
                }
            }
            return numAnalog;
        }

        /// <summary>
        /// Calculate the number of sonar ports and set the IO port used
        /// </summary>
        /// <param name="state">State to calculate information from</param>
        private void CalcSonar(VEXState state)
        {
            // Spin through the interrupt ports
            int sonarMask = 0;
            for (int i = 0; i < state.InterruptSensors.Sensors.Length; ++i)
            {
                if (state.InterruptSensors.Sensors[i].Type == SensorDefinition.SensorType.Sonar)
                {
                    // Set the bit in the mask
                    sonarMask |= 1 << i;

                    // Record the io port used
                    SetSByte(5 + i, (int) state.InterruptSensors.Sensors[i].Port);
                }
                else
                {
                    // Make sure the io port associated with the interrupt is cleared.
                    SetSByte(5 + i, 0);
                }
            }

            // Set the sonar interrupt usage mask
            SetSByte(4, sonarMask);
        }

        /// <summary>
        /// Calculate the interrupt usage mask for the encoder
        /// </summary>
        /// <param name="state">State to calculate information from</param>
        /// <returns>Integer with the bits set for the encoders that are used</returns>
        private int CalcEncoder(VEXState state)
        {
            // Spin through the interrupt sensors
            int encoderMask = 0;
            for (int i = 0; i < state.InterruptSensors.Sensors.Length; ++i)
            {
                // If it is an encoder, set the bit
                if (state.InterruptSensors.Sensors[i].Type == SensorDefinition.SensorType.Encoder)
                {
                    encoderMask |= 1 << i;
                }
            }
            return encoderMask;
        }
    }

    /// <summary>
    /// Response from setting the configuration
    /// </summary>
    [DataContract]
    public class VEXResponseSetConfiguration : VEXResponse
    {
        /// <summary>
        /// Default contructor
        /// </summary>
        public VEXResponseSetConfiguration()
            : base(VEXHelper.VEXCommandCode.ConfigurationSetCmd, 0)
        {
        }

        /// <summary>
        /// Constructor when we receive a response
        /// </summary>
        /// <param name="cmd">Received command to use the data from</param>
        public VEXResponseSetConfiguration(VEXCommand cmd)
            : base(VEXHelper.VEXCommandCode.ConfigurationSetCmd, 0)
        {
            this.Data = cmd.Data;
        }
    }

    #endregion

    #region VEX Motor control
    /// <summary>
    /// Set the value for all motor ports
    /// </summary>
    [DataContract]
    public class VEXSetAllMotors : VEXCommand
    {
        /// <summary>
        /// Default constructor
        /// Uses 8 bytes for the values
        /// </summary>
        public VEXSetAllMotors()
            : base(VEXHelper.VEXCommandCode.MotorsSetAllCmd, 8)
        {
        }

        /// <summary>
        /// Build an instance setting the motors the the values given
        /// </summary>
        /// <param name="m1">Motor 1 value</param>
        /// <param name="m2">Motor 2 value</param>
        /// <param name="m3">Motor 3 value</param>
        /// <param name="m4">Motor 4 value</param>
        /// <param name="m5">Motor 5 value</param>
        /// <param name="m6">Motor 6 value</param>
        /// <param name="m7">Motor 7 value</param>
        /// <param name="m8">Motor 8 value</param>
        public VEXSetAllMotors(int m1, int m2, int m3, int m4, int m5, int m6, int m7, int m8)
            : base(VEXHelper.VEXCommandCode.MotorsSetAllCmd, 8)
        {
            SetSByte(0, m1);
            SetSByte(1, m2);
            SetSByte(2, m3);
            SetSByte(3, m4);
            SetSByte(4, m5);
            SetSByte(5, m6);
            SetSByte(6, m7);
            SetSByte(7, m8);
        }

        /// <summary>
        /// Construct an instance from the data given 
        /// </summary>
        /// <param name="data">Data to set for each motor</param>
        public VEXSetAllMotors(byte[] data)
            : base(VEXHelper.VEXCommandCode.MotorsSetAllCmd, 8, data) { }

        /// <summary>
        /// Values assigned to the motors
        /// </summary>
        [DataMember]
        public int[] Motors
        {
            get
            {
                int[] rVal = new int[8];
                for (int i = 0; i < 8; ++i)
                {
                    rVal[i] = GetSByte(i);
                }
                return rVal;
            }
            set
            {
                for (int i = 0; i < 8; ++i)
                {
                    SetSByte(i, value[i]);
                }
            }
        }
    }
    #endregion

    #region VEX Digital/Analog input
    /// <summary>
    /// Set all of the output ports
    /// </summary>
    [DataContract]
    public class VEXSetAllPorts : VEXCommand
    {
        /// <summary>
        /// Default constructor
        /// Uses 2 bytes for the data
        /// </summary>
        public VEXSetAllPorts()
            : base(VEXHelper.VEXCommandCode.SetAllPortsCmd, 2)
        {
        }

        /// <summary>
        /// Set a bit
        /// </summary>
        /// <param name="bitNum">Bit to set (0 based)</param>
        /// <param name="value">Value to set</param>
        public void SetBit(int bitNum, bool value)
        {
            int byteNum = bitNum / 8;
            bitNum %= 8;
            base.SetBit(byteNum, bitNum, value);
        }

        /// <summary>
        /// Bits associated with the ports
        /// </summary>
        [DataMember]
        public bool[] Bits
        {
            get
            {
                bool[] rVal = new bool[16];
                for (int i = 0; i < 8; ++i)
                {
                    rVal[i] = GetBit(0, i);
                }
                for (int i = 0; i < 8; ++i)
                {
                    rVal[i+8] = GetBit(1, i);
                }
                return rVal;
            }
            set
            {
                for (int i = 0; i < 8; ++i)
                {
                    SetBit(0, i, value[i]);
                }
                for (int i = 0; i < 8; ++i)
                {
                    SetBit(1, i, value[i+8]);
                }
            }
        }
    }

    /// <summary>
    /// Get all of the data from 
    /// </summary>
    [DataContract]
    public class VEXGetAllInput : VEXCommand
    {
        /// <summary>
        /// Default constructor
        /// No data sent to the controller
        /// </summary>
        public VEXGetAllInput()
            : base(VEXHelper.VEXCommandCode.GetAllInputCmd, 0)
        {
        }
    }
    /// <summary>
    /// Response of the current input states
    /// </summary>
    [DataContract]
    public class VEXResponseGetAllInput : VEXResponse
    {
        /// <summary>
        /// Default constructor
        /// </summary>
        public VEXResponseGetAllInput()
            : base(VEXHelper.VEXCommandCode.GetAllInputCmd,
                    2 * 16 // Digital IO/Analog ports
                  + 4 * 6) // Sonar & Encoders
        {
        }

        /// <summary>
        /// Construct an instance from the result given
        /// </summary>
        /// <param name="cmd">Result to use</param>
        public VEXResponseGetAllInput(VEXCommand cmd)
            : base(VEXHelper.VEXCommandCode.GetAllInputCmd, 0)
        {
            this.Data = cmd.Data;
        }

        /// <summary>
        /// Values of the IO ports
        /// </summary>
        [DataMember]
        public ushort[] Ports
        {
            get
            {
                ushort[] rVal = new ushort[16];
                rVal[0] = GetUShort(0);
                rVal[1] = GetUShort(2);
                rVal[2] = GetUShort(4);
                rVal[3] = GetUShort(6);
                rVal[4] = GetUShort(8);
                rVal[5] = GetUShort(10);
                rVal[6] = GetUShort(12);
                rVal[7] = GetUShort(14);
                rVal[8] = GetUShort(16);
                rVal[9] = GetUShort(18);
                rVal[10] = GetUShort(20);
                rVal[11] = GetUShort(22);
                rVal[12] = GetUShort(24);
                rVal[13] = GetUShort(26);
                rVal[14] = GetUShort(28);
                rVal[15] = GetUShort(30);

                return rVal;
            }
            set
            {
                int i;
                for (i = 0; i < value.Length && i < 16; ++i)
                {
                    SetUShort(i * 2, value[i]);
                }
            }
        }

        /// <summary>
        /// Value of the interrupt ports
        /// </summary>
        [DataMember]
        public UInt32[] Interrupts
        {
            get
            {
                UInt32[] rVal = new UInt32[6];
                rVal[0] = GetUInt32(32);
                rVal[1] = GetUInt32(36);
                rVal[2] = GetUInt32(40);
                rVal[3] = GetUInt32(44);
                rVal[4] = GetUInt32(48);
                rVal[5] = GetUInt32(52);
                return rVal;
            }
            set
            {
                int i;
                for (i = 0; i < value.Length && i < 6; ++i)
                {
                    SetUInt32(32 + 4 * i, value[i]);
                }
            }
        }
    }
    #endregion

    #region VEX Encoder
    /// <summary>
    /// Reset the encoder count
    /// </summary>
    [DataContract]
    public class VEXResetEncoder : VEXCommand
    {
        /// <summary>
        /// Default constructor
        /// </summary>
        public VEXResetEncoder()
            : base(VEXHelper.VEXCommandCode.EncoderPresetCmd, 5)
        {
        }

        /// <summary>
        /// Sensor reference
        /// </summary>
        [DataMember]
        public SensorDefinition Sensor
        {
            get
            {
                SensorDefinition sensor = new SensorDefinition();
                sensor.Type = SensorDefinition.SensorType.Encoder;
                if (Data.Length >= 1)
                    sensor.Port = (SensorPort) Data[0];
                return sensor;
            }
            set
            {
                if (Data.Length >= 1)
                    Data[0] = (byte)value.Port;
            }
        }

        /// <summary>
        /// Value to preset the encoder to
        /// </summary>
        [DataMember]
        public UInt32 PresetValue
        {
            get
            {
                if (Data.Length >= 5)
                    return GetUInt32(1);
                return 0;
            }
            set
            {
                if (Data.Length >= 5)
                    SetUInt32(1, value);
            }
        }
    }
    #endregion

}
